#include "GL/openglut.h"
#include <math.h>
#include <iostream>
#include "ReconstructByKinectCommon.h"
#include "../glm/gtc/matrix_transform.hpp"

using namespace xn;
using namespace std;


extern void doFilter (IplImage* depthImage);

//---------------------------------------------------------------------------
// Defines
//---------------------------------------------------------------------------
#define INIT_WINDOW_WIDTH 800
#define INIT_WINDOW_HEIGHT 600
#define COUNT_THRESH 0
#define COUNT_LIMIT 10
#define FACE_THRESH 30.0
#define VOXEL_SIZE 500
//---------------------------------------------------------------------------
// Globals
//---------------------------------------------------------------------------
unsigned int g_winWidth;
unsigned int g_winHeight;
bool g_isPause = true;

std::vector< glm::vec3 > vertexs;
std::vector< glm::uvec3 > indices;
std::vector< glm::vec3 > normals;

IplImage * g_depthImage = NULL;

Camera g_camera;
CameraInterface g_cameraInterface (g_camera);
SensorDataInput* g_pSensorInput;

class VoxelUnit
{
public:

	std::vector< std::pair < double, int > > val;
	bool updated;

	VoxelUnit () : updated (false) {}

};

VoxelUnit g_voxel[480][640];
float g_voxelPoints[480][640];
//int g_region[6] = { -500, 500, -500, 500, -1500, -500 };
int g_region[6] = { -1000, 1000, -1000, 1000, -2000, 0 };

//---------------------------------------------------------------------------
// Code
//---------------------------------------------------------------------------
void initGlut (int argc, char* argv[]);
void initGl ();
void initCv ();
void clean ();
void drawViewport ();
void setLight ();
void updateVoxel ();
void generateMesh ();
void renderMesh ();
void renderVoxel ();


void glutIdle ()
{
	if (!g_isPause) {
		if (!g_pSensorInput->grabFrame ()) {
			clean ();
			system ("pause");
			exit (0);
		}
		if (!g_depthImage)
			g_depthImage = cvCreateImage (cvGetSize (
					g_pSensorInput->getDepthImage ()),
					g_pSensorInput->getDepthImage ()->depth, 1);
		cvCopy (g_pSensorInput->getDepthImage (), g_depthImage);
		doFilter (g_depthImage);
		updateVoxel ();
		renderVoxel ();
		generateMesh ();
	}
	g_pSensorInput->showData ("Preview - ");
	glutPostRedisplay();
}

void glutDisplay ()
{
	unsigned short frameDepth = g_pSensorInput->getSensorParameter ().frameDepth;

	// Clear the OpenGL buffers
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Setup the OpenGL viewpoint
	glViewport (0, 0, g_winWidth, g_winHeight);
	glMatrixMode (GL_PROJECTION);
	glPushMatrix ();
	glLoadIdentity ();
	gluPerspective (45, (double)g_winWidth / g_winHeight,
			1.0, frameDepth - 1);
	glMatrixMode (GL_MODELVIEW);
	glPushMatrix ();
	g_camera.setSight ();

	drawViewport ();

	setLight ();

	renderMesh ();

	glDisable (GL_NORMALIZE);
	glDisable (GL_LIGHTING);
	glDisable (GL_LIGHT0);

	glPopMatrix ();
	glMatrixMode (GL_PROJECTION);
	glPopMatrix ();
	glutSwapBuffers ();
}

void glutKeyboard (unsigned char key, int x, int y)
{
	g_cameraInterface.keyDown (key);
	switch (key) {
	case 27:
		clean ();
		exit (0);
	case 'p':
		g_isPause = !g_isPause;
	}
}

void glutMouse (int button, int state, int x, int y)
{
	if (state == GLUT_DOWN)
		g_cameraInterface.mouseDown (button, x, y);
	else
		g_cameraInterface.mouseUp (button, x, y);
}

void glutMotion (int x, int y)
{
	g_cameraInterface.mouseDrag (x, y);
}

void glutMouseWheel (int button, int direction, int x, int y)
{
	g_cameraInterface.mouseWheel (direction, x, y);
}

void glutReshape (int w, int h)
{
	g_winWidth = w;
	g_winHeight = h;
}

int main(int argc, char* argv[])
{
	if (argc != 3) {
		printf ("Command line argument error.\n");
		return -1;
	}
	try {
		if (strcmp (argv[1], "kinect") == 0)
			g_pSensorInput = new SensorDataKinectInput (argv[2]);
		else if (strcmp (argv[1], "file") == 0)
			g_pSensorInput = new SensorDataFileInput (argv[2]);
		else {
			printf ("Command line argument error.\n");
			return -1;
		}

		initGlut (argc, argv);
		initGl ();
		initCv ();
		glutMainLoop ();
	} catch (...) {
		printf ("Exception caught\n");
	}
	return 0;
}

void initGlut (int argc, char* argv[])
{
	glutInit (&argc, argv);
	glutInitDisplayMode (GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutInitWindowSize (g_winWidth = INIT_WINDOW_WIDTH,
			g_winHeight = INIT_WINDOW_HEIGHT);
	glutCreateWindow ("Reconstruct By Kinect");

	glutKeyboardFunc (glutKeyboard);
	glutMouseFunc (glutMouse);
	glutMotionFunc (glutMotion);
	glutDisplayFunc (glutDisplay);
	glutIdleFunc (glutIdle);
	glutReshapeFunc (glutReshape);
	glutMouseWheelFunc (glutMouseWheel);
}

void initGl ()
{
	glEnable (GL_DEPTH_TEST);
// 	glEnable (GL_COLOR_MATERIAL);

	g_camera.setDistance (30);
	g_camera.setViewpoint (glm::vec3 (0, 0, -1000));
}

void initCv ()
{
}

void clean ()
{
	delete g_pSensorInput;
}

void drawViewport ()
{
	double frameVFOV = g_pSensorInput->getSensorParameter ().frameVFOV;
	double frameHFOV = g_pSensorInput->getSensorParameter ().frameHFOV;
	unsigned short frameDepth = g_pSensorInput->getSensorParameter ().frameDepth;

	glm::dmat4x4 perspectiveMat = glm::perspective< double > (
			glm::degrees (frameVFOV), frameHFOV / frameVFOV, 1.0, frameDepth - 1);
	glm::dmat4x4 inversePerspectiveMat = glm::inverse (perspectiveMat);
	glm::dvec4 t1 = inversePerspectiveMat * glm::dvec4 (-1, -1, -1.0, 1.0);
	glm::dvec4 t2 = inversePerspectiveMat * glm::dvec4 (1, -1, -1.0, 1.0);
	glm::dvec4 t3 = inversePerspectiveMat * glm::dvec4 (1, 1, -1.0, 1.0);
	glm::dvec4 t4 = inversePerspectiveMat * glm::dvec4 (-1, 1, -1.0, 1.0);
	glm::dvec4 t5 = inversePerspectiveMat * glm::dvec4 (-1, -1, 1.0, 1.0);
	glm::dvec4 t6 = inversePerspectiveMat * glm::dvec4 (1, -1, 1.0, 1.0);
	glm::dvec4 t7 = inversePerspectiveMat * glm::dvec4 (1, 1, 1.0, 1.0);
	glm::dvec4 t8 = inversePerspectiveMat * glm::dvec4 (-1, 1, 1.0, 1.0);

	glm::dvec4 a1 = inversePerspectiveMat * glm::dvec4 (-1, -1, 0.3, 1.0);
	glm::dvec4 a2 = inversePerspectiveMat * glm::dvec4 (1, -1, 0.3, 1.0);
	glm::dvec4 a3 = inversePerspectiveMat * glm::dvec4 (1, 1, 0.3, 1.0);
	glm::dvec4 a4 = inversePerspectiveMat * glm::dvec4 (-1, 1, 0.3, 1.0);
	glColor3f (1.0f, 1.0f, 1.0f);
	glBegin (GL_LINES);
	glVertex4dv (&t1[0]); glVertex4dv (&t2[0]);
	glVertex4dv (&t2[0]); glVertex4dv (&t3[0]);
	glVertex4dv (&t3[0]); glVertex4dv (&t4[0]);
	glVertex4dv (&t4[0]); glVertex4dv (&t1[0]);

	glVertex4dv (&t5[0]); glVertex4dv (&t6[0]);
	glVertex4dv (&t6[0]); glVertex4dv (&t7[0]);
	glVertex4dv (&t7[0]); glVertex4dv (&t8[0]);
	glVertex4dv (&t8[0]); glVertex4dv (&t5[0]);

	glVertex4dv (&t1[0]); glVertex4dv (&t5[0]);
	glVertex4dv (&t2[0]); glVertex4dv (&t6[0]);
	glVertex4dv (&t3[0]); glVertex4dv (&t7[0]);
	glVertex4dv (&t4[0]); glVertex4dv (&t8[0]);

	glVertex4dv (&a1[0]); glVertex4dv (&a2[0]);
	glVertex4dv (&a2[0]); glVertex4dv (&a3[0]);
	glVertex4dv (&a3[0]); glVertex4dv (&a4[0]);
	glVertex4dv (&a4[0]); glVertex4dv (&a1[0]);
	glEnd ();

	glPushMatrix ();
	glTranslatef (0, 0, (g_region[4] + g_region[5]) / 2);

	glPushAttrib (GL_CURRENT_BIT);
	glColor3f (1.0, 0.5, 0.5);

	glutWireCube (g_region[5] - g_region[4]);

	glPopAttrib ();
	glPopMatrix ();
}

void generateMesh ()
{
	IplImage* depthImage = g_pSensorInput->getDepthImage ();
	int frameWidth = g_pSensorInput->getSensorParameter ().frameWidth;
	int frameHeight = g_pSensorInput->getSensorParameter ().frameHeight;
	double halfFrameWidth = g_pSensorInput->getSensorParameter ().frameWidth / 2.0;
	double halfFrameHeight = g_pSensorInput->getSensorParameter ().frameHeight / 2.0;
	double fy =  glm::tan (g_pSensorInput->getSensorParameter ().frameVFOV / 2);
	double fx = glm::tan (g_pSensorInput->getSensorParameter ().frameHFOV / 2);

	vertexs.resize (frameWidth * frameHeight);
	normals.resize (frameWidth * frameHeight);
	indices.clear ();
	for (unsigned int x = 0, i = 0; x < frameWidth; ++x)
		for (unsigned int y = 0; y < frameHeight; ++y, ++i) {
			//float depth = g_voxelPoints[y][x];
			float depth = CV_IMAGE_ELEM (depthImage, unsigned short, y, x);
			vertexs[i] = glm::vec3 ((x / halfFrameWidth - 1) * depth * fx,
					(1 - y / halfFrameHeight) * depth * fy, -depth);
			if (x >= 1 && y >= 1) {
				unsigned int cornerIndices[] = { i - frameHeight - 1,
						i - 1, i, i - frameHeight };
				if (abs (vertexs[cornerIndices[0]].z - vertexs[cornerIndices[3]].z) < FACE_THRESH &&
						abs (vertexs[cornerIndices[3]].z - vertexs[cornerIndices[1]].z) < FACE_THRESH &&
						abs (vertexs[cornerIndices[1]].z - vertexs[cornerIndices[0]].z) < FACE_THRESH &&
						vertexs[cornerIndices[0]].z && vertexs[cornerIndices[3]].z && vertexs[cornerIndices[1]].z)
					indices.push_back (glm::uvec3 (cornerIndices[0], cornerIndices[3], cornerIndices[1]));
				if (abs (vertexs[cornerIndices[1]].z - vertexs[cornerIndices[3]].z) < FACE_THRESH &&
						abs (vertexs[cornerIndices[3]].z - vertexs[cornerIndices[2]].z) < FACE_THRESH &&
						abs (vertexs[cornerIndices[2]].z - vertexs[cornerIndices[1]].z) < FACE_THRESH &&
						vertexs[cornerIndices[1]].z && vertexs[cornerIndices[3]].z && vertexs[cornerIndices[2]].z)
					indices.push_back (glm::uvec3 (cornerIndices[1], cornerIndices[3], cornerIndices[2]));
			}
			normals[i] = glm::dvec3 ();
		}
	for (unsigned int i = 0; i < indices.size (); ++i) {
		glm::vec3 faceNormal = glm::normalize (glm::cross (
				vertexs[indices[i][2]] - vertexs[indices[i][1]],
				vertexs[indices[i][0]] - vertexs[indices[i][1]]));
		normals[indices[i][0]] += faceNormal;
		normals[indices[i][1]] += faceNormal;
		normals[indices[i][2]] += faceNormal;
	}
	for (size_t i = 0; i < normals.size (); ++i)
		normals[i] = glm::normalize (normals[i]);
}

void renderMesh ()
{
	if (indices.size () == 0)
		return;
//	glDisable (GL_LIGHTING);
	glEnableClientState (GL_VERTEX_ARRAY);
	glEnableClientState (GL_NORMAL_ARRAY);
// 	glEnableClientState (GL_COLOR_ARRAY);
	glVertexPointer (3, GL_FLOAT, 0, &vertexs[0][0]);
 	glNormalPointer (GL_FLOAT, 0, &normals[0][0]);
//	glColorPointer (3, GL_FLOAT, 0, &normals[0][0]);
	glDrawElements (GL_TRIANGLES, indices.size () * 3, GL_UNSIGNED_INT, &indices[0][0]);
//	glDisable (GL_LIGHTING);
// 	glColor3f (1.0, 0.0, 0.0);
// 	glDrawArrays (GL_POINTS, 0, vertexs.size ());
	glDisableClientState (GL_VERTEX_ARRAY);
 	glDisableClientState (GL_NORMAL_ARRAY);
// 	glDisableClientState (GL_COLOR_ARRAY);
}

void setLight ()
{
	glEnable (GL_NORMALIZE);
	glEnable (GL_LIGHTING);
	glEnable (GL_LIGHT0);
	float lp[] = { 1.0f, 1.0f, 1.0f, -1.0f };
	float lc[] = { 1.0f, 1.0f, 1.0f, 1.0f };
	glLightfv (GL_LIGHT0, GL_POSITION, lp);
	glLightfv (GL_LIGHT0, GL_AMBIENT, lc);
	glLightfv (GL_LIGHT0, GL_DIFFUSE, lc);
	glLightModeli (GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);
}

void updateVoxel ()
{
	unsigned int frameWidth = g_pSensorInput->getSensorParameter ().frameWidth;
	unsigned int frameHeight = g_pSensorInput->getSensorParameter ().frameHeight;
	unsigned short frameDepth = g_pSensorInput->getSensorParameter ().frameDepth;

	for (unsigned int v = 0; v < frameHeight; ++v)
		for (unsigned int u = 0; u < frameWidth; ++u)
			g_voxel[v][u].updated = false;

	for (unsigned int x = 0; x < frameWidth; ++x) {
		for (unsigned int y = 0; y < frameHeight; ++y) {
			glm::dvec3 p (x, y, CV_IMAGE_ELEM (g_depthImage, unsigned short, y, x));
			bool used = false;
			for (std::vector< std::pair< double, int > >::iterator iter =
					g_voxel[y][x].val.begin (); iter != g_voxel[y][x].val.end ();){
				if (abs (iter->first - p.z) < 20.0 && !used) {
					//double d = p.z - iter->first;
					//iter->first += (1.0 - 1.0 / 20.0 * abs (d)) / 20.0 * d;
					iter->first = (iter->first * iter->second + p.z) / (iter->second + 1);
					++iter->second;
					if (iter->second >= COUNT_LIMIT)
						iter->second = COUNT_LIMIT;
					used = true;
					++iter;
					
				} else {
					iter->second -= 1;
					if (iter->second <= 0)
						iter = g_voxel[y][x].val.erase (iter);
					else
						++iter;
				}
			}
			if (!used)
				g_voxel[y][x].val.push_back (make_pair (p.z, 1));
			g_voxel[y][x].updated = true;
		}
	}

// 	for (unsigned int v = 0; v < VOXEL_SIZE; ++v) {
// 		for (unsigned int u = 0; u < VOXEL_SIZE; ++u) {
// 			if (!g_voxel[v][u].updated) {
// 				double sumZ = 0.0;
// 				double countZ = 0;
// 				double minZ = 0.0;
// 				double maxZ = -10000.0;
// 				int neighIndexList[][2] = {{u - 1, v - 1}, {u, v - 1},
// 						{u + 1, v - 1}, {u + 1, v}, {u + 1, v + 1},
// 						{u, v + 1}, {u - 1, v + 1}, {u - 1, v}};
// 				for (int i = 0; i < 8; ++i) {
// 					int nx = neighIndexList[i][0];
// 					int ny = neighIndexList[i][1];
// 					if (nx >= 0 && nx < VOXEL_SIZE && ny >= 0 &&
// 							ny < VOXEL_SIZE && g_voxel[nx][ny].updated) {
// 						int maxCount = -1;
// 						double maxCountZ = -10000.0;
// 						for (size_t i = 0; i < g_voxel[ny][nx].val.size (); ++i) {
// 							if (g_voxel[ny][nx].val[i].second > maxCount) {
// 								maxCount = g_voxel[ny][nx].val[i].second;
// 								maxCountZ = g_voxel[ny][nx].val[i].first;
// 							}
// 						}
// 						if (maxCount != -1) {
// 							sumZ += maxCountZ;
// 							++countZ;
// 							if (maxCountZ < minZ)
// 								minZ = maxCountZ;
// 							if (maxCountZ > maxZ)
// 								maxZ = maxCountZ;
// 						}
// 					}
// 				}
// 				double avgZ = (maxZ - minZ < FACE_THRESH ? sumZ / countZ : -10000.0);
// 				bool used = false;
// 				for (std::vector< std::pair< double, int > >::iterator iter =
// 						g_voxel[v][u].val.begin ();
// 						iter != g_voxel[v][u].val.end ();){
// 					if (abs (iter->first - avgZ) < 20.0 && !used) {
// 						double d = avgZ - iter->first;
// 						iter->first += (1.0 - 1.0 / 20.0 * abs (d)) / 20.0 * d;
// 						++iter->second;
// 						used = true;
// 						++iter;
// 					} else {
// 						iter->second -= 1;
// 						if (iter->second <= 0)
// 							iter = g_voxel[v][u].val.erase (iter);
// 						else
// 							++iter;
// 					}
// 				}
// 				if (!used)
// 					g_voxel[v][u].val.push_back (make_pair (avgZ, 0));
// 				g_voxel[v][u].updated = true;
// 			}
// 		}
// 	}
}

void renderVoxel ()
{
	for (int v = 0; v < 480; ++v) {
		for (int u = 0; u < 640; ++u) {
			int maxCount = -1;
			double maxCountZ = 10000.0;
			for (size_t i = 0; i < g_voxel[v][u].val.size (); ++i) {
				if (g_voxel[v][u].val[i].second >= COUNT_THRESH &&
						g_voxel[v][u].val[i].second > maxCount) {
					maxCount = g_voxel[v][u].val[i].second;
					maxCountZ = g_voxel[v][u].val[i].first;
				}
			}
			g_voxelPoints[v][u] = maxCountZ;
		}
	}
}

